/*
 * Helper functions, have no idea where to put them so let them be here
 * 
 */
using System;
using IrrlichtNETCP;
using Ode.NET;

namespace kinematix
{

	public class kHelp
	{
		
		public kHelp()
		{
		}
		
		public static Vector3D Quat2Rot(d.Quaternion q)
		{
			float w, x, y, z;
			Vector3D euler = new Vector3D();
			w = q.W;
			x = q.X;
			y = q.Y;
			z = q.Z;
			double sqw = w * w;
			double sqx = x * x;
			double sqy = y * y;
			double sqz = z * z;
			euler.Z = (float) (Math.Atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw))*180.0f/3.14f);
			euler.X = (float) (Math.Atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw))*180.0f/3.14f);
			euler.Y = (float) (Math.Asin(-2.0 * (x*z - y*w))*180.0f/3.14f);
			return euler;
		}
		
		public static bool CheckForDisabling (IntPtr b, float lin_vel, float ang_vel)
		{
			bool disable = true;
			d.Vector3 lvel = d.BodyGetLinearVel(b);
			float linear_speed = lvel.X * lvel.X + lvel.Y * lvel.Y +
				lvel.Z * lvel.Z;
			if (linear_speed > lin_vel)
				disable = false;
			d.Vector3 avel = d.BodyGetAngularVel(b);
			float angular_speed = avel.X * avel.X + avel.Y * avel.Y +
				avel.Z * avel.Z;
			if (angular_speed > ang_vel)
				disable = false;
			return disable;
		}		
		
		
	}
}